/* ***********************************************************************
* image_stab_if_K.cpp
* ABOUT : Implement the class of Image stabilization of K-team.
* AUTHOR : Dilawar Singh
* Created on : April 28, 2010
* Modified on : 
(c) Dilawar Singh, 2010
*************************************************************************/

#include "types.h"
#include "img_stab_if_K.h"
#include <iostream>
#include <vector>
#include <cmath>

video_handler* img_stab_if_K::pBaseVideoHandler;


/*
* Constructor.
*/
img_stab_if_K::img_stab_if_K(void):myQuatsVec(), myFilteredVec(4), myVecS(WINDOW_SIZE)
{
    for(int i=0; i < 2*QUAT_LENGTH; i++)
    {
		arrayTwoQuaternion[i] = 0;
    }

	myFilteredVec.resize(4);
	myFilteredVec[0] = 0; myFilteredVec[1] = 0;
	myFilteredVec[2] = 0; myFilteredVec[3] = 0;
	

   // pObjCamMotionManager = new cam_motion_mgr();
}
img_stab_if_K::~img_stab_if_K()
{
    if(pObjCamMotionManager)
	delete pObjCamMotionManager;
}

//Set Base Interface at startup
int img_stab_if_K::initInterface(video_handler* pVideoHandler)
{
    pBaseVideoHandler = pVideoHandler;
    return(true);
}

/*
img_stab_if_K::img_stab_if_K(kalmanType *pArrayQuat, kalmanType *pArrayCamMat) : myQuatsVec(4*WINDOW_SIZE), myFilteredVec(4), myVecS(WINDOW_SIZE)
{
    printf("Class IS: Enterning explicit constructor\n");

    for(int i = 0; i < 2*QUAT_LENGTH; i++)
    {
        arrayTwoQuaternion[i] = *pArrayQuat;
        //printf("%lf",arrayTwoQuaternion[i]); // Debug
        pArrayQuat++;
    }

    printf("\n");
    for(int i=0; i < (2*K_MATRIX_LENGTH); i++)
    {
        arrayCamInternalMatrix[i] = *pArrayCamMat;
        //printf("%lf\t",arrayCamInternalMatrix[i]); //Debug
        pArrayCamMat++;
    }
}

*/

/******************************************************************************
 * This function will accumulate the quaternion in a queue.
 *****************************************************************************/
vector<kalmanType> img_stab_if_K::accQuatsInQueue(vector<kalmanType> myQuatsVec
                                                    , kalmanType* pQuaternion)
{

#if IS_DEBUG     
    prints("\nEntering accQuatsInQueue function.\n");
    for(vector<kalmanType>::size_type  i =0; i < myQuatsVec.size() ; i++)
    {
        prints("\t myQuatsVec[%d] : %lf", i, myQuatsVec[i]);
    }
#endif

    if(myQuatsVec.size() > 2*QUAT_LENGTH)
    {
        while(myQuatsVec.size() > 2*QUAT_LENGTH)
        {
            myQuatsVec.erase(myQuatsVec.begin());
        }
    }
    for(int i =0; i < QUAT_LENGTH; i++)
    {
        myQuatsVec.push_back(*pQuaternion);
        IsQuaternionK::kcQuaternion[i] = *pQuaternion;
        pQuaternion++;
    }


    if(myQuatsVec.size() > 2*QUAT_LENGTH)
    {
        for(vector<kalmanType>::size_type  i = 0; i < 4 ; i++)
        {
            kalmanType a = ALPHA; // Change this value in img_stab_if_K.h file.
            myQuatsVec[2*QUAT_LENGTH + i ] = a*myQuatsVec[2*QUAT_LENGTH + i] 
            + (1-a)*myQuatsVec[QUAT_LENGTH + i];
        }

        kalmanType* pA = &myQuatsVec[0];
        pA = pA + 2*QUAT_LENGTH;
        for(int i = 0; i < 4; i++)
        {
            lpfQuaternion[i] = *pA;
            pA++;
        }

    }



#if IS_DEBUG
    // Tested with Matlab.
    prints("\n\r--DEBUG: print array...\n");
    prints("\n\r**************************************");
    for(vector<kalmanType>::size_type  i =0; i < myQuatsVec.size() ; i++)
    {
        prints("\t myQuatsVec[%d] : %lf", i ,myQuatsVec[i]);
    }
    prints("\n\r";
    for(int i = 0; i < 4; i++)
    {
        prints("\tIsQuaternionK::kcQuaternion[%d] : %lf", i
		  ,IsQuaternionK::kcQuaternion[i]);
    }
    prints("\n\r");
    for(int i = 0; i < 4; i++)
    {
        prints("IsQuaternionK::lpfQuaternion[%d] : %lf" , i,
		  IsQuaternionK::lpfQuaternion[i]);
    }

#endif

    return myQuatsVec;
}

/* 
 * Run image Stabilize 
 */
 void img_stab_if_K::runImageStabilizeK(void)
 {
   //int cnt;

   //ULONG lastVsyncTimeStamp;
   //ULONG lastMotionTimeStamp;

     /* Here we update the arrayTwoQuaternions */
   for(int i = 0; i < QUAT_LENGTH; i ++)
   {
       arrayTwoQuaternion[i] = arrayTwoQuaternion[QUAT_LENGTH + i]; // pervious Quaternion
                                                              // = current Quaternion
   }

   /*
    * Here, push the corrected quaternion into queue. This queue always contains current 
    * quaternion as well as previous quaternion.
    */
   
   for(int i = 0; i < QUAT_LENGTH; i++)
   {
       arrayTwoQuaternion[QUAT_LENGTH + i] = pObjCamMotionManager->pObjKalmanFilter->correctedQuaternion_win[i];
   }
   
    /* 
     * If there is no change in quaternions. return.
    */
    for(int i = 0; i < QUAT_LENGTH; i++)
    {
    	if(arrayTwoQuaternion[i] == arrayTwoQuaternion[QUAT_LENGTH+i])
		return;
     }
    
   /* Here we push the quaternion into the vector of quaternions to be low
    * passed filtered using K algorithm.
    * THIS WILL UPDATE lpfQuaternion AND kcQuaternion VARIABLES.
    */
   
   myQuatsVec = img_stab_if_K::accQuatsInQueue(myQuatsVec,
           pObjCamMotionManager->pObjKalmanFilter->correctedQuaternion_win);

 
   /*
     TO DO: Extrapolate the quaternion using time stamp
   */


   /*
   * Create an array of camera matrix.
   */
   for(int i = 0; i < K_MATRIX_LENGTH; i++)
   {
       arrayCamInternalMatrix[i] = arrayCamInternalMatrix[K_MATRIX_LENGTH + i];
   }
   for(int i = 0; i < K_MATRIX_LENGTH; i++)
   {
       arrayCamInternalMatrix[K_MATRIX_LENGTH + i] = pObjCamMotionManager->pObjCamMotion->camInternalMat[i];
   }

    /*
     * VARIOUS FUNCTIONS
    */
   
      /*
    * This function will return the Homography Matrix.
    * ret : imgHomographyMat;
    */
   //kalmanType* pA;
   //kalmanType* pQ;
       
   IsHomographyK::getHomographyMat(kcQuaternion, lpfQuaternion, arrayCamInternalMatrix);
   IsHomographyK::decomposeHomographyMat(IsHomographyK::IS_HomographyMat);
#if 0
   printf("Printing values...\n");
   prints(endl<<"imgS:"<<IS.imgSThetaTArray[0];
   prints(endl<<"imgTheta:"<<IS.imgSThetaTArray[1];
   prints(endl<<"imgT:"<<IS.imgSThetaTArray[2]<<"\t"<<IS.imgSThetaTArray[3];
#endif

   /*
    * Print values here for Debugging.
    */
 }
